﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading.Tasks;

namespace ImageTools
{
    public class DitherRGB565
    {
        
        private static readonly byte[] DitherThresholdR =
        [
            1, 7, 3, 5, 0, 8, 2, 6,
            7, 1, 5, 3, 8, 0, 6, 2,
            3, 5, 0, 8, 2, 6, 1, 7,
            5, 3, 8, 0, 6, 2, 7, 1,
            0, 8, 2, 6, 1, 7, 3, 5,
            8, 0, 6, 2, 7, 1, 5, 3,
            2, 6, 1, 7, 3, 5, 0, 8,
            6, 2, 7, 1, 5, 3, 8, 0
        ];

        private static readonly byte[] DitherThresholdG = 
        [
        1, 3, 2, 2, 3, 1, 2, 2,
        2, 2, 0, 4, 2, 2, 4, 0,
        3, 1, 2, 2, 1, 3, 2, 2,
        2, 2, 4, 0, 2, 2, 0, 4,
        1, 3, 2, 2, 3, 1, 2, 2,
        2, 2, 0, 4, 2, 2, 4, 0,
        3, 1, 2, 2, 1, 3, 2, 2,
        2, 2, 4, 0, 2, 2, 0, 4
        ];

        private static readonly byte[] DitherThresholdB = 
        [
        5, 3, 8, 0, 6, 2, 7, 1,
        3, 5, 0, 8, 2, 6, 1, 7,
        8, 0, 6, 2, 7, 1, 5, 3,
        0, 8, 2, 6, 1, 7, 3, 5,
        6, 2, 7, 1, 5, 3, 8, 0,
        2, 6, 1, 7, 3, 5, 0, 8,
        7, 1, 5, 3, 8, 0, 6, 2,
        1, 7, 3, 5, 0, 8, 2, 6
        ];
        private static byte ClosestRB(byte c) => (byte)((c >> 3) << 3);
        private static byte ClosestG(byte c) => (byte)((c >> 2) << 2);

        public static byte DitherXY_R(int x, int y, byte rb)
        {
            int thresholdId = ((y & 7) << 3) + (x & 7);
            rb = ClosestRB((byte)Math.Min(rb + DitherThresholdR[thresholdId], 0xFF));

            return rb;
        }

        public static byte DitherXY_G(int x, int y, byte g)
        {
            int thresholdId = ((y & 7) << 3) + (x & 7);
            g = ClosestG((byte)Math.Min(g + DitherThresholdG[thresholdId], 0xFF));

            return g;
        }

        public static byte DitherXY_B(int x, int y, byte b)
        {
            int thresholdId = ((y & 7) << 3) + (x & 7);
            b = ClosestRB((byte)Math.Min(b + DitherThresholdB[thresholdId], 0xFF));

            return b;
        }

        public DitherRGB565(int width, int height, byte[] rgbBytes)
        {
            for (int y=0; y < height; y++)
            {
                for (int x=0; x < width; x++)
                {
                    int index = (y * width + x) * 3;
                    rgbBytes[index] = DitherXY_R(x, y, rgbBytes[index]); // R
                    rgbBytes[index + 1] = DitherXY_G(x, y, rgbBytes[index + 1]); // G
                    rgbBytes[index + 2] = DitherXY_B(x, y, rgbBytes[index + 2]); // B

                    rgbBytes[index] &= 0xF8;
                    rgbBytes[index + 1] &= 0xFC;
                    rgbBytes[index + 2] &= 0xF8;
                }
            }
        }
    }
}

